package physics;

/**
 * @author Zach Turk
 * @author Ken "David" McClain
 */

import java.io.Serializable;

import main.SceneGraphNode;
import shape.Circle;
import shape.Compound;
import shape.Polygon;
import shape.Square;
import shape.Triangle;

public class PhysicsObject implements Serializable {
	/** Serial ID */
	private static final long serialVersionUID = -8835568058236261970L;
	
	public float inverseMass;
	public Vector2f position;
	public Vector2f velocity;
	public Vector2f acceleration;
	public Vector2f centerOfMass;
	public float inverseMomentOfInertia;
	public float orientation;
	public float angularVelocity;
	public SceneGraphNode renderable;
	
	public PhysicsObject() {
		inverseMass = 1;
		position = new Vector2f();
		velocity = new Vector2f();
		acceleration = new Vector2f();
		centerOfMass = new Vector2f();
		// inverseMomentOfInertia needs to be set in subclasses
	}

	public void updateState(float timePeriod) {
		position.sumScale(velocity, timePeriod);
		position.sumScale(acceleration, timePeriod * timePeriod / 2);
		velocity.sumScale(acceleration, timePeriod);
		orientation += angularVelocity * timePeriod;
		clearCaches();
	}
	
	public void updateRenderable() {
		if (renderable != null) {
			renderable.translateX = position.x;
			renderable.translateY = position.y;
			renderable.rotation = (float)(180 * orientation / Math.PI);
		}
	}

	public void clearCaches() {
	}

	public CollisionInfo getCollision(PhysicsObject other) {
		if (inverseMass == 0 && other.inverseMass == 0)
			return null;
		if (this instanceof HalfSpace && other instanceof Circle)
			return getCollision((HalfSpace)this, (Circle)other);
		if (this instanceof Circle && other instanceof HalfSpace) {
			CollisionInfo cInfo = getCollision((HalfSpace)other, (Circle)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		if (this instanceof HalfSpace && other instanceof Triangle)
			return getCollision((HalfSpace)this, (Triangle)other);
		if (this instanceof Triangle && other instanceof HalfSpace) {
			CollisionInfo cInfo = getCollision((HalfSpace)other, (Triangle)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		if (this instanceof HalfSpace && other instanceof Square)
			return getCollision((HalfSpace)this, (Square)other);
		if (this instanceof Square && other instanceof HalfSpace) {
			CollisionInfo cInfo = getCollision((HalfSpace)other, (Square)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		if (this instanceof Circle && other instanceof Circle)
			return getCollision((Circle)this, (Circle)other);
		if (this instanceof Circle && other instanceof Triangle)
			return getCollision((Circle)this, (Triangle)other);
		if (this instanceof Triangle && other instanceof Circle) {
			CollisionInfo cInfo = getCollision((Circle)other, (Triangle)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		if (this instanceof Circle && other instanceof Square)
			return getCollision((Circle)this, (Square)other);
		if (this instanceof Square && other instanceof Circle) {
			CollisionInfo cInfo = getCollision((Circle)other, (Square)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
			
		if (this instanceof Triangle && other instanceof Triangle)
			return getCollision((Triangle)this, (Triangle)other);
		if (this instanceof Triangle && other instanceof Square)
			return getCollision((Triangle) this, (Square)other);
		if (this instanceof Square && other instanceof Triangle) {
			CollisionInfo cInfo = getCollision((Triangle)other, (Square)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		if (this instanceof Square && other instanceof Square)
			return getCollision((Square)this, (Square)other);
		
		if (this instanceof HalfSpace && other instanceof Compound)
			return getCollision((HalfSpace)this, (Compound)other);
		if (this instanceof Compound && other instanceof HalfSpace) {
			CollisionInfo cInfo = getCollision((HalfSpace)other, (Compound)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		
		if (this instanceof Circle && other instanceof Compound)
			return getCollision((Circle)this, (Compound)other);
		if (this instanceof Compound && other instanceof Circle) {
			CollisionInfo cInfo = getCollision((Circle)other, (Compound)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}

		if (this instanceof Triangle && other instanceof Compound)
			return getCollision((Triangle)this, (Compound)other);
		if (this instanceof Compound && other instanceof Triangle) {
			CollisionInfo cInfo = getCollision((Triangle)other, (Compound)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		
		if (this instanceof Square && other instanceof Compound)
			return getCollision((Square)this, (Compound)other);
		if (this instanceof Compound && other instanceof Square) {
			CollisionInfo cInfo = getCollision((Square)other, (Compound)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		
		if (this instanceof Compound && other instanceof Compound)
			return getCollision((Compound)this, (Compound)other);
		
		return null;
	}
	
	private static CollisionInfo getCollision(HalfSpace a, Circle b) {
		float distance = a.normal.dot(b.position);
		distance -= a.intercept;
		if (distance >= b.radius)
			return null;
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = b.radius - distance;
		cInfo.normal = new Vector2f(a.normal);
		cInfo.positionA = new Vector2f(b.position);
		cInfo.positionA.sumScale(cInfo.normal, -b.radius + cInfo.depth);
		cInfo.positionB = new Vector2f(b.position);
		cInfo.positionB.sumScale(cInfo.normal, -b.radius);
		return cInfo;
	}
	
	private static CollisionInfo getCollision(Circle a, Circle b) {
		Vector2f tmp = new Vector2f(b.position);
		tmp.sumScale(a.position, -1);
		float distance = tmp.length() - a.radius - b.radius;
		if (distance >= 0)
			return null;
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = -distance;
		try {
			tmp.normalize();
		} catch (IllegalStateException ise) { }
		cInfo.normal = tmp;
		cInfo.positionA = new Vector2f(a.position);
		cInfo.positionA.sumScale(cInfo.normal, a.radius);
		cInfo.positionB = new Vector2f(b.position);
		cInfo.positionB.sumScale(cInfo.normal, -b.radius);
		return cInfo;
	}
	
	private static CollisionInfo getCollision(HalfSpace a, Triangle b) {
		Vector2f[] vertices = b.getVertices();
		float[] distances = new float[vertices.length];
		
		for (int i = 0; i < vertices.length; i++)
			distances[i] = a.normal.dot(vertices[i]) - a.intercept;
		
		int minIndex = 0;
		for (int i = 1; i < distances.length; i++)
			if (distances[i] < distances[minIndex])
				minIndex = i;
		if (distances[minIndex] >= 0)
			return null;
		
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = -distances[minIndex];
		cInfo.normal = new Vector2f(a.normal);
		cInfo.positionA = new Vector2f(vertices[minIndex]);
		cInfo.positionA.sumScale(cInfo.normal, cInfo.depth);
		cInfo.positionB = new Vector2f(vertices[minIndex]);
		return cInfo;
	}
	
	private static CollisionInfo getCollision(Circle a, Polygon b) {
		Vector2f[] vertices = b.getVertices();
		Vector2f[] normals = b.getNormals();
		float[] distances = new float[vertices.length];

		for (int i = 0; i < vertices.length; i++) {
			Vector2f tmp = new Vector2f(a.position);
			tmp.sumScale(vertices[i], -1);
			distances[i] = tmp.dot(normals[i]) - a.radius;

			// project circle position onto vertex
			Vector2f dot = new Vector2f(a.position);
			dot.sumScale(normals[i], -tmp.dot(normals[i]));

			// Don't need to do a range-check if dot is 'inside' vertex.
			if (distances[i] + a.radius <= 0) continue;

			// Dot is the point on the line, let's see if it's within the range of vertices.
			if (dot.x < Math.min(vertices[i].x, vertices[(i+1) % vertices.length].x) ||
					dot.x > Math.max(vertices[i].x, vertices[(i+1) % vertices.length].x) ||
					dot.y < Math.min(vertices[i].y, vertices[(i+1) % vertices.length].y) ||
					dot.y > Math.max(vertices[i].y, vertices[(i+1) % vertices.length].y) ) {

				// Get distance between circle position and next vertex
				Vector2f tmp2 = new Vector2f(a.position);
				tmp2.sumScale(vertices[(i+1) % vertices.length], -1);
				if (tmp.length() < tmp2.length()) {
					distances[i] = tmp.length() - a.radius;
					tmp.normalize();
					normals[i] = tmp;
				} else {
					distances[i] = tmp2.length() - a.radius;
					tmp2.normalize();
					normals[i] = tmp2;
				}
				b.clearCaches();
			}

		}

		int maxIndex = 0;
		// Calculate maximum distance penetrated
		for (int i = 1; i < distances.length; i++) {
			if (distances[i] > distances[maxIndex])
				maxIndex = i;
		}

		// If we did not penetrate at all, no collision
		if (distances[maxIndex] >= 0)
			return null;

		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = -distances[maxIndex];
		cInfo.normal = new Vector2f(normals[maxIndex]);
		cInfo.normal.scale(-1);
		cInfo.positionA = new Vector2f(a.position);
		cInfo.positionA.sumScale(cInfo.normal, a.radius + cInfo.depth / 2);
		cInfo.positionB = new Vector2f(a.position);
		cInfo.positionB.sumScale(cInfo.normal, a.radius - cInfo.depth / 2);
		return cInfo;
	}

	private static CollisionInfo getCollision(Triangle a, Triangle b) {
		return sat(a, b);
	}
	
	private static CollisionInfo getCollision(Triangle a, Square b) {
		return sat(a, b);
	}
	
	private static CollisionInfo getCollision(Square a, Square b) {
		return sat(a, b);
	}
	
	private static CollisionInfo sat(Polygon a, Polygon b) {
		Vector2f[] a_n = a.getNormals();
		Vector2f[] b_n = b.getNormals();
		Vector2f[] a_v = a.getVertices();
		Vector2f[] b_v = b.getVertices();
		Vector2f closestPoint = null;
		float min_overlap = Float.MAX_VALUE;
		intervalData tmp;
		
		CollisionInfo c = new CollisionInfo();
		
		// check to see if a has a non intersecting axis
		for (int i = 0; i < a_n.length; i++) {
			if ((tmp = separatedByAxis(a_n[i], a, b, new Vector2f[] {a_v[i], a_v[(i+1)%a_v.length]})) == null)
				return null;
			else if (tmp.depth < min_overlap) {
				min_overlap = tmp.depth;
				closestPoint = tmp.collisionPoint;
				
				c.normal = new Vector2f(a_n[i]);
				c.depth = tmp.depth;
			}
		}
		
		// check to see if b has a non intersecting axis
		for (int i = 0; i < b_n.length; i++) {
			if ((tmp = separatedByAxis(b_n[i], b, a, new Vector2f[] {b_v[i], b_v[(i+1)%b_v.length]})) == null)
				return null;
			else if (tmp.depth < min_overlap) {
				min_overlap = tmp.depth;
				closestPoint = tmp.collisionPoint;

				c.normal = new Vector2f(b_n[i]);
				c.normal.scale(-1);
				c.depth = tmp.depth;
			}
		}
		
		if (closestPoint == null)
			return null;
		c.positionA = new Vector2f(closestPoint);
		c.positionA.sumScale(c.normal, c.depth);
		c.positionB = new Vector2f(closestPoint);
		return c;
	}
	
	private static intervalData separatedByAxis(Vector2f n, Polygon a, Polygon b, Vector2f[] a_vert) {
		intervalData a_data = interval(n, a);
		intervalData b_data = interval(n, b);
		if (a_data.max < b_data.min || b_data.max < a_data.min)
			return null;
		intervalData tmp = new intervalData();
		if (Math.abs(a_data.max - b_data.min) < Math.abs(b_data.max - a_data.min)) {
			if (!a_data.maxVertex.equals(a_vert[0]) && !a_data.maxVertex.equals(a_vert[1]))
				tmp.depth = Float.MAX_VALUE;
			else
				tmp.depth = (a_data.max - b_data.min);
			tmp.collisionPoint = new Vector2f(b_data.minVertex);
		} else {
			if (!a_data.minVertex.equals(a_vert[0]) && !a_data.minVertex.equals(a_vert[1]))
				tmp.depth = Float.MAX_VALUE;
			else
				tmp.depth = (b_data.max - a_data.min);
			tmp.collisionPoint = new Vector2f(b_data.maxVertex);
		}
		return tmp;
	}
	
	private static intervalData interval(Vector2f n, Polygon s) {
		// set up initial data
		intervalData data = new intervalData();
		Vector2f[] vertices = s.getVertices();
		data.min = data.max = vertices[0].dot(n);
		data.minVertex = data.maxVertex = new Vector2f(vertices[0]);
		
		// find the local extremes
		for (int i = 0; i < vertices.length; i++) {
			float d = vertices[i].dot(n);
			if (d < data.min) {
				data.min = d;
				data.minVertex = new Vector2f(vertices[i]);
			}
			else if (d > data.max) {
				data.max = d;
				data.maxVertex = new Vector2f(vertices[i]);
			}
		}
		return data;
	}
	
	private static CollisionInfo getCollision(HalfSpace a, Square b) {
		Vector2f[] vertices = b.getVertices();
		float[] distances = new float[vertices.length];
		
		for (int i = 0; i < vertices.length; i++)
			distances[i] = a.normal.dot(vertices[i]) - a.intercept;
		
		int minIndex = 0;
		for (int i = 1; i < distances.length; i++)
			if (distances[i] < distances[minIndex])
				minIndex = i;
		if (distances[minIndex] >= 0)
			return null;
		
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = -distances[minIndex];
		cInfo.normal = new Vector2f(a.normal);
		cInfo.positionA = new Vector2f(vertices[minIndex]);
		cInfo.positionA.sumScale(cInfo.normal, cInfo.depth);
		cInfo.positionB = new Vector2f(vertices[minIndex]);
		return cInfo;
	}
	
	private static CollisionInfo getCollision(HalfSpace a, Compound b) {
		PhysicsObject[] po = b.getObjects();
		CollisionInfo cInfo = null;
		for (int i = 0; i < po.length; i++) {
			if (po[i] == null) continue;
			CollisionInfo c = a.getCollision(po[i]);
			if (c == null) continue;
			if (cInfo == null || cInfo.depth < c.depth)
				cInfo = c;
		}
		return cInfo;
	}
	
	private static CollisionInfo getCollision(Circle a, Compound b) {
		PhysicsObject[] po = b.getObjects();
		CollisionInfo cInfo = null;
		for (int i = 0; i < po.length; i++) {
			if (po[i] == null) continue;
			CollisionInfo c = a.getCollision(po[i]);
			if (c == null) continue;
			if (cInfo == null || cInfo.depth < c.depth)
				cInfo = c;
		}
		if (cInfo != null) {
			return cInfo;
		}
		return cInfo;
	}
	
	private static CollisionInfo getCollision(Triangle a, Compound b) {
		PhysicsObject[] po = b.getObjects();
		CollisionInfo cInfo = null;
		for (int i = 0; i < po.length; i++) {
			if (po[i] == null) continue;
			CollisionInfo c = a.getCollision(po[i]);
			if (c == null) continue;
			if (cInfo == null || cInfo.depth < c.depth)
				cInfo = c;
		}
		return cInfo;
	}
	
	private static CollisionInfo getCollision(Square a, Compound b) {
		PhysicsObject[] po = b.getObjects();
		CollisionInfo cInfo = null;
		for (int i = 0; i < po.length; i++) {
			if (po[i] == null) continue;
			CollisionInfo c = a.getCollision(po[i]);
			if (c == null) continue;
			if (cInfo == null || cInfo.depth < c.depth)
				cInfo = c;
		}
		return cInfo;
	}
	
	private static CollisionInfo getCollision(Compound a, Compound b) {
		PhysicsObject[] poa = a.getObjects();
		PhysicsObject[] pob = b.getObjects();
		CollisionInfo cInfo = null;
		
		for (int i = 0; i < poa.length; i++) {
			
			for (int j = 0; j < pob.length; j++) {
				if (poa[i] == null || pob[j] == null) continue;
				
				CollisionInfo c = poa[i].getCollision(pob[j]);
				if (c == null) continue;
				if (cInfo == null || cInfo.depth < c.depth)
					cInfo = c;
			}
		}
		return cInfo;
	}
	
	public void resolveCollision(PhysicsObject other, CollisionInfo cInfo) {
		// Calculate the velocity of the collision point on the calling object.
		Vector2f relativeCollisionPositionA = new Vector2f(cInfo.positionA);
		relativeCollisionPositionA.sumScale(position, -1);
		relativeCollisionPositionA.sumScale(centerOfMass, -1);
		Vector2f linearVelocityA = new Vector2f(-relativeCollisionPositionA.y, relativeCollisionPositionA.x);
		linearVelocityA.scale(angularVelocity);
		linearVelocityA.sum(velocity);
		// Calculate the velocity of the collision point on the other object.
		Vector2f relativeCollisionPositionB = new Vector2f(cInfo.positionB);
		relativeCollisionPositionB.sumScale(other.position, -1);
		relativeCollisionPositionB.sumScale(other.centerOfMass, -1);
		Vector2f linearVelocityB = new Vector2f(-relativeCollisionPositionB.y, relativeCollisionPositionB.x);
		linearVelocityB.scale(other.angularVelocity);
		linearVelocityB.sum(other.velocity);
		// Calculate the relative velocity between the calling object and
		// other object, as if the calling object were stationary and only
		// the other object were moving.
		Vector2f relativeVelocity = new Vector2f(linearVelocityB);
		relativeVelocity.sumScale(linearVelocityA, -1);
		// Calculate the component of the relative velocity that lays along
		// the collision normal.
		float compRelVelAlongNormal = relativeVelocity.dot(cInfo.normal);
		// Calculate the resulting impulse per unit mass.
		float impulse = (float)(2.0 * compRelVelAlongNormal / (
				inverseMass + other.inverseMass + 
				Math.pow(relativeCollisionPositionA.cross(cInfo.normal), 2) * inverseMomentOfInertia +
				Math.pow(relativeCollisionPositionB.cross(cInfo.normal), 2) * other.inverseMomentOfInertia));
		// Adjust the linear and angular velocities of each object in proportion
		// to their effective masses.
		velocity.sumScale(cInfo.normal, impulse * inverseMass);
		other.velocity.sumScale(cInfo.normal, -impulse * other.inverseMass);
		angularVelocity += relativeCollisionPositionA.cross(cInfo.normal) * impulse * inverseMomentOfInertia;
		other.angularVelocity -= relativeCollisionPositionB.cross(cInfo.normal) * impulse * other.inverseMomentOfInertia;

		// Calculate the amount of object overlap per unit mass.
		float depth = (float)(cInfo.depth / (
				inverseMass + other.inverseMass + 
				Math.pow(relativeCollisionPositionA.cross(cInfo.normal), 2) * inverseMomentOfInertia +
				Math.pow(relativeCollisionPositionB.cross(cInfo.normal), 2) * other.inverseMomentOfInertia));
		// Adjust the position and orientation  of each object in proportion
		// to their effective masses to remove overlap.
		position.sumScale(cInfo.normal, -depth * inverseMass);
		other.position.sumScale(cInfo.normal, depth * other.inverseMass);
		orientation -= relativeCollisionPositionA.cross(cInfo.normal) * depth * inverseMomentOfInertia;
		other.orientation += relativeCollisionPositionB.cross(cInfo.normal) * depth * other.inverseMomentOfInertia;
		
		clearCaches();
		other.clearCaches();
	}
	
	
	public static class intervalData {
		public float min;
		public float max;
		public float depth;
		public Vector2f minVertex;
		public Vector2f maxVertex;
		public Vector2f collisionPoint;
		
		public intervalData() {};
	}
}
